package org.scalpel.mainApps;

import java.text.DecimalFormat;
import java.text.NumberFormat;
import java.util.ArrayList;
import java.util.List;

import org.scalpel.HHObjects.MapManager;
import org.scalpel.common.KeyPoint;
import org.scalpel.locodo.Locomotion;

import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.core.Scalar;

import android.graphics.Canvas;
import android.graphics.Color;
import android.graphics.Paint;
import android.graphics.Path;

/**
 * 
 * Mobile inherit from Locomotion and add graphic representation 
 * of the robot. 
 * It also displays the robot projection on the obstacle map image.
 *
 */
public class Mobile extends Locomotion {

	// Shape of the robot
  private List<KeyPoint> fShape;
  private List<Point> fShapeCvPoint = null;
  
  
  private double fConvMeterToPix;
  NumberFormat fNumberFormat = null;
  
  public Mobile(String name, List<KeyPoint> polygon, double encoderWheelPerimeter,
      double encoderWheelsDistanceToCenter, int encoderTickPerRotation, double convMeterToPix) {
    
    super(name, encoderWheelPerimeter, encoderWheelsDistanceToCenter,
        encoderTickPerRotation);
    
    fShape = polygon;
    fConvMeterToPix = convMeterToPix;
    fNumberFormat = new DecimalFormat("0.000");

    fShapeCvPoint = new ArrayList<Point>(fShape.size());
    for(KeyPoint p : fShape){
      fShapeCvPoint.add(new Point(new double[]{p.x,p.y}));
    }      

  }

  public void drawNoObstacleShape(Canvas canvas, Paint paint, Mat probaImg, int zoomFactor){
    org.scalpel.common.KeyPoint pos = getPosition();
    
    int i = 0;
    for(KeyPoint p : fShape){
      
      Point r = MapManager.rotationPoint(p.x+pos.x, p.y + pos.y, pos, pos.a, fConvMeterToPix/zoomFactor);
      fShapeCvPoint.get(i).x = r.x;
      fShapeCvPoint.get(i++).y = r.y;
    }      

    Core.fillConvexPoly(probaImg, fShapeCvPoint, new Scalar(0) /*BLACK*/, 16 /*CV_AA*/, 0);
  }
  
  /**
   * draw the robot and debug string to the main canvas
   * @param canvas reference to the main canvas
   * @param robotPaint current robotPaint
   */
  public void onDraw(Canvas canvas, Paint robotPaint){
  	
    ///////////// Draw the robot /////////////
    KeyPoint pos = getPosition();
    KeyPoint target = getTarget();
    double prec = getDistancePrecision();
    double[] realSpeed = getRealSpeed();
    double[] speed = getSpeed();
    
    // Meter to pixels conversion
    int px = (int)(pos.x * fConvMeterToPix);
    int py = (int)(pos.y * fConvMeterToPix);

    int pTargetx = (int)(target.x * fConvMeterToPix);
    int pTargety = (int)(target.y * fConvMeterToPix);

    // DRAW
    canvas.save();     //================>
    canvas.translate(px, py);
    canvas.rotate((float)( -pos.a * 180.0 / Math.PI ));

    // draw robot body
    Path robotPath = new Path();
    boolean first = true; 
    for(KeyPoint p : fShape){
      if( first ){
        robotPath.moveTo(  (int)(p.x * fConvMeterToPix), 
                          (int)(p.y * fConvMeterToPix));
        first = false;
        continue;
      }
      robotPath.lineTo( (int)(p.x * fConvMeterToPix), 
                        (int)(p.y * fConvMeterToPix));
    }
    robotPaint.setColor(Color.DKGRAY);
    canvas.drawPath(robotPath, robotPaint);
    
    // draw the wheel with the real speed vector
    int x1 = (int) (fEncoderWheelsDistanceToCenter * fConvMeterToPix);
    int x2 = x1 - 2; // 2 pix width
    int y1 = (int) ((fEncoderWheelDiameter / 2.0) * fConvMeterToPix);
    int y2 = -y1; 
    
    robotPaint.setColor(Color.BLUE);
    canvas.drawRect( y1, x1, y2, x2, robotPaint);
    canvas.drawRect( -1, x1, 1, x1 + (float)(realSpeed[1]/2.), robotPaint);
    canvas.drawRect( -2, x1, -1, x1 + (float)(speed[1]/0.02), robotPaint);
    
    robotPaint.setColor(Color.GRAY);
    canvas.drawRect( y1, -x2, y2, -x1, robotPaint);
    canvas.drawRect( -1, -x2, 1, -x2 - (float)(realSpeed[0]/2.), robotPaint);
    canvas.drawRect( -2, -x2, -1, -x2 - (float)(speed[0]/0.02), robotPaint);
    
    
    // draw the robot center
    robotPaint.setColor(Color.YELLOW);
    canvas.drawCircle(0, 0, 1, robotPaint);

    canvas.restore(); //<================
    

    ///////////// Draw string info and the target ///////////// 
    canvas.save();     //================>
    // pos feedback
    robotPaint.setColor(Color.WHITE);
    
    final int connectedPosX = 672;

    String posString = "p " + fNumberFormat.format(pos.x) + " " + fNumberFormat.format(pos.y) + " " + fNumberFormat.format(pos.a);
    canvas.drawText(posString, connectedPosX+10, 10, robotPaint);
    posString = "speed " + fNumberFormat.format(speed[0]) + " " + fNumberFormat.format(speed[1]);
    canvas.drawText(posString, connectedPosX+10, 20, robotPaint);
    posString = "reals " + fNumberFormat.format(realSpeed[0]) + " " + fNumberFormat.format(realSpeed[1]);
    canvas.drawText(posString, connectedPosX+10, 30, robotPaint);
    robotPaint.setStyle(Paint.Style.STROKE);
    if( isTargetReached() )
    	robotPaint.setColor(Color.RED);
    else
    	robotPaint.setColor(Color.GREEN);
    canvas.drawCircle(pTargetx, pTargety, (float) (prec * fConvMeterToPix), robotPaint);
    robotPaint.setStyle(Paint.Style.FILL);
    canvas.restore(); //<================
    
  }

}
